# -*- coding: utf-8 -*-
import numpy
import time
from copy import deepcopy

import numpy as np
from naoqi import qi
from PID import  PID
import cv2

class Hough():
    def __init__(self):
        self.ses = qi.Session()
        self.ses.connect("tcp://192.168.61.115:9559")
        self.video = self.ses.service("ALVideoDevice")
        self.post=self.ses.service("ALRobotPosture")
        self.motion=self.ses.service("ALMotion")
        # self.post.goToPosture("Crouch", 0.5)
        self.motion.setAngles(["HeadYaw","HeadPitch"],[0,np.pi/7],0.2)
        self.subscriber = self.video.subscribeCamera("demo", 1, 2, 13, 60)
    def get_frame(self):
        imageNAO = self.video.getImageRemote(hough.subscriber)
        if imageNAO == None:
            print('cannot capture.')
            return None
        elif imageNAO[6] == None:
            print('no image data string.')
            return None
        else:
            frameArray = np.frombuffer(imageNAO[6], dtype=np.uint8).reshape(
                [imageNAO[1], imageNAO[0], imageNAO[2]])
            self.pre_frame=frameArray
        return frameArray
    def line_det(self):
            frameArray=self.get_frame()
            # 转变灰度值
            image_gray = cv2.cvtColor(frameArray, cv2.COLOR_BGR2GRAY)
            # 转换二值图像
            retval, image_thre = cv2.threshold(image_gray, 200, 255, cv2.THRESH_BINARY)
            # 高斯滤波
            image_thre = cv2.GaussianBlur(image_thre, (3, 3), 1)
            # 形态学变换
            kernel = np.ones((3, 3), np.uint8)
            image_thre = cv2.morphologyEx(image_thre, cv2.MORPH_CLOSE, kernel)
            image_thre = cv2.morphologyEx(image_thre, cv2.MORPH_OPEN, kernel)
            # 轮廓提取
            contours = cv2.Canny(image_thre, 190, 300)
            lines_ci = cv2.HoughLines(contours, 1, np.pi / 180, 175)
            print lines_ci
            return lines_ci
    def draw(self,lines_ci):
        result=deepcopy(self.pre_frame)
        result_all=deepcopy(result)
        if lines_ci is not None:
            for line in lines_ci:
                _, theta = line[0]
                rho, theta = line[0]
                a = np.cos(theta)
                b = np.sin(theta)
                x0 = a * rho
                y0 = b * rho
                x1 = int(x0 + 1000 * (-b))
                y1 = int(y0 + 1000 * (a))
                x2 = int(x0 - 1000 * (-b))
                y2 = int(y0 - 1000 * (a))
                cv2.line(result, (x1, y1), (x2, y2), (0, 0, 255), 2)
                if 0< theta < np.pi /4 or 3*np.pi/4 <theta< np.pi:
                    cv2.line(result_all, (x1, y1), (x2, y2), (0, 0, 255), 2)
            cv2.imshow("pepper-top-camera-line_ci", result)
            cv2.imshow("pepper-top-camera-line_ALL", result_all)
    def direction(self,lines_ci):
        # 检测结果，并输出方向
        if lines_ci is not None:
            for line in lines_ci:
                _, theta = line[0]
                print theta
                if theta < np.pi / 4 and theta > 3 * np.pi / 4:
                    theta = theta - 0.33
                    if theta <0.0 : theta=0.01
                if theta < np.pi / 4:
                    print "right"
                elif theta > np.pi * 3 / 4:
                    print "left"


hough=Hough()
pid=PID(-2,0,0)
try :
    while True:
        lines_ci=hough.line_det()
        hough.draw(lines_ci)
        theta_mean=0.33
        count=0
        if lines_ci is not None:
            for line in lines_ci:
                _,theta=line[0]
                if 0< theta < np.pi /4 or 3*np.pi/4 <theta< np.pi:
                    if count==0:
                        theta_mean=theta
                    else:
                        theta_mean += theta
                    print theta
                    count+=1
            print count
            if count!=0:
                theta_mean=theta_mean/count
        print theta_mean
        control = pid.update( theta_mean- 0 )
        print control
        time.sleep(0.05)
        print type(control)
        if isinstance(control, np.float64):
            control=numpy.float(control)
        # hough.motion.moveToward(0.4,0,control)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    hough.video.releaseImage(hough.subscriber)
    hough.video.unsubscribe(hough.subscriber)
'''
try:
    while True:
        imageNAO = hough.video.getImageRemote(hough.subscriber)

        if imageNAO == None:
            print('cannot capture.')
        elif imageNAO[6] == None:
            print('no image data string.')
        else:
            frameArray = np.frombuffer(imageNAO[6], dtype=np.uint8).reshape(
                [imageNAO[1], imageNAO[0], imageNAO[2]])
            #转变灰度值
            image_gray = cv2.cvtColor(frameArray, cv2.COLOR_BGR2GRAY)
            result = deepcopy(frameArray)
            #转换二值图像
            retval,image_thre=cv2.threshold(image_gray,200,255, cv2.THRESH_BINARY)
            #高斯滤波
            image_thre=cv2.GaussianBlur(image_thre,(3,3),1)
            #形态学变换
            kernel = np.ones((3, 3), np.uint8)
            image_thre = cv2.morphologyEx(image_thre, cv2.MORPH_CLOSE, kernel)
            image_thre = cv2.morphologyEx(image_thre, cv2.MORPH_OPEN, kernel)
            #轮廓提取
            contours=cv2.Canny(image_thre,190,300)

            #暂时轮廓提取结果
            cv2.imshow("pepper-top-camera-asdf", contours)
            #hough变换提取线条。
            lines = cv2.HoughLinesP(contours, 1, np.pi / 180, 175)

            lines_ci = cv2.HoughLines(contours, 1, np.pi / 180, 175)
            print lines_ci
            if lines is not None:
                for line in lines:
                    x1, y1, x2, y2 = line[0]
                    cv2.line(result, (x1, y1), (x2, y2), (0, 255, 0), 2)
            cv2.imshow("pepper-top-camera-lines", result)
            # 绘制直线
            if lines_ci is not None:
                for line in lines_ci:
                    rho, theta = line[0]
                    a = np.cos(theta)
                    b = np.sin(theta)
                    x0 = a * rho
                    y0 = b * rho
                    print x0,y0
                    x1 = int(x0 + 1000 * (-b))
                    y1 = int(y0 + 1000 * (a))
                    x2 = int(x0 - 1000 * (-b))
                    y2 = int(y0 - 1000 * (a))+
                    cv2.line(result, (x1, y1), (x2, y2), (0, 0, 255), 2)
                cv2.imshow("pepper-top-camera-line_ci", result)
            # print lines
            print np.shape(lines_ci)
            time.sleep(1)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            hough.video.releaseImage(hough.subscriber)
            hough.video.unsubscribe(hough.subscriber)
            break
finally :

'''